#include "Communication/SubImu.h"
#include "modules/CtrlModule/ActivePlanner/CliffFollowPlanner.h"
#include "data_struct/base/DataSet.h"
#include "data_struct/robot/ImuData.h"
#include "std_msgs/msg/u_int8.hpp"

namespace behavior_controller {
ImuData DataSet::imu_data;
namespace SubImu {
static rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imuSub;

void sub_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {

    static bool set = DataSet::imu_data.SetFrame(msg->header.frame_id);
    DataSet::imu_data.updateData(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z,
                                 msg->linear_acceleration.x, msg->linear_acceleration.y,
                                 msg->linear_acceleration.z,
                                 msg->orientation.x, msg->orientation.y, msg->orientation.z,
                                 msg->orientation.w);
}

void SubImuInfo(rclcpp::Node::SharedPtr &node_handle) {
    imuSub = node_handle->create_subscription<sensor_msgs::msg::Imu>(
            "imu", rclcpp::SensorDataQoS(), sub_callback);
    std::cout << "SubImu SubImuInfo" << std::endl;
}
} // namespace SubImu

} // namespace behavior_controller